#include "facing.h"
#include "math_core.h"
#include "graphics/graphics_util.h"
#include <math.h>

using namespace math;

Facing::Facing(float	TurnSpeed , float InitialAngle ) : 
	m_TurnSpeed(TurnSpeed),
	m_CurrentAngle(InitialAngle),
	m_TargetAngle(InitialAngle),
	m_EnableSnap(true)
{

}

Facing::~Facing()
{

}

float	Facing::GetRemainder() const
{
	if ((float)fabs(m_TargetAngle - m_CurrentAngle) > 180.0f) return fabs(m_CurrentAngle-m_TargetAngle);
	return fabs(m_TargetAngle - m_CurrentAngle);
}

void	Facing::Tick(float dt)
{
	float	TurnSpeed = dt * m_TurnSpeed;

	if ((float)fabs(m_TargetAngle - m_CurrentAngle) > 180.0f) 
		TurnSpeed = -TurnSpeed;

	if ((float)fabs(m_TargetAngle - m_CurrentAngle) < (float)fabs(TurnSpeed)) 
		m_CurrentAngle = m_TargetAngle; 
	else if (m_TargetAngle > m_CurrentAngle)	 
		m_CurrentAngle+=TurnSpeed; 
	else
		m_CurrentAngle-=TurnSpeed; 

	if (m_EnableSnap && fabs(m_TargetAngle - m_CurrentAngle)  < fabs(m_TurnSpeed))
	{
		m_CurrentAngle=m_TargetAngle;
	} 

	if (m_CurrentAngle > 360) 
		m_CurrentAngle-= 360; 

	else if (m_CurrentAngle < 0 ) 
		m_CurrentAngle+= 360;

	if (m_TargetAngle > 360) 
		m_TargetAngle-= 360; 

	else if (m_TargetAngle < 0 ) 
		m_TargetAngle+= 360;
}

float	Facing::FindDeltaAngle(float from, float to)
{
	float delta = to - from;
	if (fabs(delta) > 180.0f) delta = 360-delta;
	return delta;
}

void	Facing::SetCurrent(float f)
{
	m_CurrentAngle = m_TargetAngle = f;
}

void	Facing::SetTarget(float f)
{
	m_TargetAngle = f;

}

float	Facing::AngleBetweenVectors(const math::Vec2& V1, const math::Vec2& V2)
{
	return ((float)(atan2(V2.y, V2.x)-atan2(V1.y, V1.x)) * RAD_TO_DEG);
}

float	Facing::AngleFromTo(const math::Vec2& From, const math::Vec2& To)
{
	return 180.0f + ((float)atan2(To.x-From.x, To.y-From.y) * RAD_TO_DEG);
}

float	Facing::AngleVector(const math::Vec2& Dir)
{
	return 180.0f + ((float)atan2(Dir.x, Dir.y) * RAD_TO_DEG);
}

math::Vec2	Facing::VectorFromAngle(float Angle)
{
	float	RadRotation = Angle * DEG_TO_RAD;
	return math::Vec2(	-sinf(RadRotation), -sinf(RadRotation+ MATH_HALF_PI));
}

math::Vec2	Facing::GetForwardW2H() const 
{
	math::Vec2	Dir = VectorFromAngle(GetAngle());
	Dir.y *= GraphicsUtil::W2H;
	return Dir;
}

math::Vec2	Facing::GetForward() const 
{
	return VectorFromAngle(GetAngle());
}

math::Vec2	Facing::GetRight() const 
{
	float	RadRotation = GetAngle() * DEG_TO_RAD;
	return math::Vec2(	 -sinf(RadRotation+ MATH_HALF_PI),  -sinf(RadRotation+ (MATH_HALF_PI*2.0f)));
}
